/*
 * Created by juangeng on 21/08/19.
 *  rl_c1_driver 机器人底盘控制与tcp/ip连接
 *  ________________________________
 *  ip_tcp_connect
 *  负责上位机与android平板通过tcp/ip协议进行连接；
 *  接收来自INS_probe模块的实时定位数据；
 *  接收来自自动控制程序wmr_plan的控制指令；
 *  接收来自android平板的目标点，转发给wmr_plan程序；
 *  将规划的路径发送给android平板。
 * */

#include "ip_tcp_connect.h"

std::ofstream out_file_;
std::mutex file_lock_;

std::string double2string(const double value) {
    std::ostringstream out;
    out.precision(15);
    out << value;
    return out.str();
}

void Server::run() {
    cmd_pub_ = node_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    //target point : -deg, deg, m
    Pose2D_pub_ = node_.advertise<geometry_msgs::Pose2D>("/target_pose", 10);
    ros::Subscriber location_sub     = node_.subscribe<insprobe_msgs::Navigation>("/insprobe/nav0", 1, &Server::getLocationMsg, this);
    ros::Subscriber android_path_sub = node_.subscribe<nav_msgs::Path>("/path_android", 10, &Server::pathPointCallback, this);
    ros::Subscriber batteryV_sub     = node_.subscribe<std_msgs::Float32>("/battery_V", 1, &Server::batteryVCallback, this);
    ros::Subscriber batteryP_sub     = node_.subscribe<std_msgs::Float32>("/battry_percent", 1, &Server::batteryPCallback, this);

    ros::Timer send_baterry = node_.createTimer(ros::Duration(1), &Server::sendBattery, this);

    ros::spin();

}

void Server::batteryVCallback(const std_msgs::Float32::ConstPtr &msg) {
    voltage_ = msg->data;
}

void Server::batteryPCallback(const std_msgs::Float32::ConstPtr &msg) {
    battery_ = msg->data;
}

/*
 * nav_msgs::Path msg poses
 * -- BLH (deg, deg, m)
 * */
void Server::pathPointCallback(const nav_msgs::Path::ConstPtr &msg) {
    long count;
    sockmutex_.lock();
    count = socket_.use_count();

    if (!count) {
        sockmutex_.unlock();
        return;
    }

    if (socket_.get()->is_open()){
        auto& path = msg->poses;
        for (int i = 0; i < (int)path.size(); ++i) {
            auto& pos = path[i].pose.position;
            int length = sprintf(tcptxbuff_, "$PATH,%d,%0.8lf,%0.8lf,%d\n", i, pos.x, pos.y, 3);
            socket_.get()->write_some(boost::asio::buffer(tcptxbuff_, (size_t) length));
        }
    }
    sockmutex_.unlock();
}

void Server::getLocationMsg(const insprobe_msgs::Navigation::ConstPtr &msg) {
    insprobe_pack_.week     = msg->week;
    insprobe_pack_.weeksec  = msg->weeksec;
    insprobe_pack_.lat      = msg->latitude;
    insprobe_pack_.lon      = msg->longitude;
    insprobe_pack_.alt      = msg->altitude;
    insprobe_pack_.pitch    = msg->pitch;
    insprobe_pack_.roll     = msg->roll;
    insprobe_pack_.yaw      = msg->yaw;
    insprobe_pack_.veldown  = msg->velocity_ground;
    insprobe_pack_.veleast  = msg->velocity_east;
    insprobe_pack_.velnorth = msg->velocity_north;
    insprobe_pack_.navmode  = msg->nav_state;

    if(out_file_) {
        file_lock_.lock();
        out_file_ << "$insprobe,"
        << double2string(insprobe_pack_.week) << ","
        << double2string(insprobe_pack_.weeksec) << ","
        << double2string(insprobe_pack_.lat) << ","
        << double2string(insprobe_pack_.lon) << ","
        << double2string(insprobe_pack_.alt) << ","
        << double2string(insprobe_pack_.pitch) << ","
        << double2string(insprobe_pack_.roll) << ","
        << double2string(insprobe_pack_.yaw) << ","
        << double2string(insprobe_pack_.veldown) << ","
        << double2string(insprobe_pack_.veleast) << ","
        << double2string(insprobe_pack_.velnorth) << ","
        << double2string(insprobe_pack_.navmode) << std::endl;
        file_lock_.unlock();
    }

    if ((insprobe_pack_.weeksec - (int) insprobe_pack_.weeksec) < 0.02) {
        /* 检查是否查连接 */
        long count;
        sockmutex_.lock();
        count = socket_.use_count();
        sockmutex_.unlock();
        if (!count)
            return;

        sockmutex_.lock();
        if (socket_.get()->is_open()) {
            int length = sprintf(tcptxbuff_,
                    "$POS,%0.3f,%0.8lf,%0.8lf,%0.2f,%0.2f,%0.2f,%0.2f,%0.8lf,%0.8lf,%0.8lf,%d\n",
                    insprobe_pack_.weeksec,
                    insprobe_pack_.lat,  insprobe_pack_.lon,   insprobe_pack_.alt,
                    insprobe_pack_.velnorth, insprobe_pack_.veleast, insprobe_pack_.veldown,
                    insprobe_pack_.roll, insprobe_pack_.pitch, insprobe_pack_.yaw,
                    insprobe_pack_.navmode);

            socket_.get()->write_some(boost::asio::buffer(tcptxbuff_, (size_t) length));
        }
        sockmutex_.unlock();
    }
}

void Server::sendBattery(const ros::TimerEvent &event) {
    /* 1S定时器 */

    /* 检是否查连接 */
    long count;
    sockmutex_.lock();
    count = socket_.use_count();
    sockmutex_.unlock();
    if (!count)
        return;

    sockmutex_.lock();
    if (socket_.get()->is_open()) {
        msgmutex_.lock();

        int length = sprintf(tcptxbuff_, "$POWER,%0.2f,%0.2f\n", battery_, voltage_);
        msgmutex_.unlock();

        socket_.get()->write_some(boost::asio::buffer(tcptxbuff_, (size_t) length));
    }
    sockmutex_.unlock();
}

void Server::serverThread() {
    acceptor_ = new ip::tcp::acceptor(ioservice_, ip::tcp::endpoint(ip::tcp::v4(), (unsigned short)tcpport_));

    startAccept();

    std::cout << "start listen on tcp port " << tcpport_ << std::endl;
    ioservice_.run();
}

void Server::startAccept() {
    /* 开始监听, 连接成功转移到回调函数 */
    socket_ptr sock(new ip::tcp::socket(ioservice_));
    acceptor_->async_accept(*sock, boost::bind(&Server::acceptHandler, this, sock, _1));
}

void Server::acceptHandler(socket_ptr sock, const boost::system::error_code &err) {
    if (err) {
        ROS_ERROR( "accept error" );
        return;
    }

    ROS_INFO("connected");

    sockmutex_.lock();
    socket_.reset();
    socket_ = std::move(sock);
    sockmutex_.unlock();
    startRead();

    /* 继续监听 */
    startAccept();
}

void Server::startRead() {
    sockmutex_.lock();
    socket_.get()->async_receive(boost::asio::buffer(tcprxbuff_), boost::bind(&Server::readHandler, this, _1, _2));
    sockmutex_.unlock();
}

void Server::readHandler(const boost::system::error_code &err, std::size_t bytes) {

    if (err) {
        ROS_ERROR("disconnected");
        sockmutex_.lock();
        socket_.get()->close();
        socket_.reset();
        sockmutex_.unlock();
        return;
    }

    /* 数据缓存 */
    memcpy(tcprxtemp_ + tcprxcnt_, tcprxbuff_, bytes);
    tcprxcnt_ += bytes;

//    ROS_INFO("%s", tcprxtemp_);
    if ((tcprxtemp_[0] == '$') && (tcprxtemp_[tcprxcnt_ - 1] == '\n')) {
        /* 清除行尾, 保证使用strtod正确 */
        tcprxtemp_[tcprxcnt_ - 1] = '\0';

        char *ptr;
        ptr = strtok(tcprxtemp_, ",");
        if (!strcmp(ptr, "$VEL")) {

            ptr = strtok(nullptr, ",");
            current_twist_.linear.x = strtod(ptr, nullptr);

            ptr = strtok(nullptr, ",");
            current_twist_.angular.z = strtod(ptr, nullptr);

            cmd_pub_.publish(current_twist_);
            //theta用来存储控制模式: 0-handle; 1 -auto; 2 -loop
            target_pose2d_.x = 0;
            target_pose2d_.y = 0;
            target_pose2d_.theta = 0;    //手动模式
            Pose2D_pub_.publish(target_pose2d_);

            /* 收到完整数据, 清空缓存大小 */
            tcprxcnt_ = 0;
        }
        else if (!strcmp(ptr, "$AUTO")) {
            /* 目标位置lat */
            ptr = strtok(nullptr, ",");
            target_lat_ = strtod(ptr, nullptr);

            ptr = strtok(nullptr, ",");
            target_lon_ = strtod(ptr, nullptr);
            //发布目标点与模式
            target_pose2d_.x = target_lat_;
            target_pose2d_.y = target_lon_;
            //theta用来存储控制模式: 0-handle; 1 -auto; 2 -loop
            target_pose2d_.theta = 1;
            Pose2D_pub_.publish(target_pose2d_);
            /* 收到完整数据, 清空缓存大小 */
            tcprxcnt_ = 0;
        }
        else if(!strcmp(ptr, "$LOOP")){
            /*巡逻模式，传入多个目标点，一次接收一个*/
            ptr = strtok(nullptr, ",");
            target_lat_ = strtod(ptr, nullptr);

            ptr = strtok(nullptr, ",");
            target_lon_ = strtod(ptr, nullptr);
            /* 收到完整数据, 清空缓存大小 */
            tcprxcnt_ = 0;
        }
    } else {
        if (tcprxcnt_ > MAX_TCP_SIZE) {
            tcprxcnt_ = 0;
        }
        ROS_WARN("received a incomplete tcp package");
    }

    startRead();
}

//这里用指针，后续可以考虑换成智能指针
Server *server_;

int main(int argc, char** argv){
    ros::init(argc, argv, "tcp_connect");
    ros::NodeHandle node;
    ros::NodeHandle private_node("~");

    /* 设置参数 */
    int tcpport;
    private_node.param<int>("tcp_port",        tcpport,       9999);

    std::string file_path;
    private_node.param<std::string>("record/file_path", file_path,
                          std::string("/home/jg/Desktop/document/wmr_nav/"));

    /* 设置文件名称 */
    std::string time_name, file_name;
    std::ostringstream ostr;
    //获取当前系统的当前日期/时间
    std::time_t now = std::time(0);
    std::tm *ltm = std::localtime(&now);
    /**********file name about time************/
    ostr << 1 + ltm->tm_mon << "_" << ltm->tm_mday << "_" << ltm->tm_hour << "_"
         << ltm->tm_min << "_" << ltm->tm_sec << ".chassis";

    time_name = ostr.str();
    file_name = file_path + time_name;

    out_file_.open(file_name.c_str());
    if (!out_file_) {
        std::cerr << "[rl_c1_driver]: open output file error !" << std::endl;
        exit(-1);
    }
    /* 设置文件名称 */

    /* 建立“server” 负责通过tcp/ip传输信息，通过ros msg向wmr_plan交流 */
    server_ = new Server(tcpport, node);

    /* 开始连接 */
    server_->run();

    return 0;
}
